#include "collide.h"
#include "swagme.h"
#include "TouchableObject.h"
#include <iostream>
#include <cmath>
using std::cout;
using std::cerr;
using std::endl;

TouchableObject* CollisionMap::atCol(int x, int y)
{
  if(x < 0 || y < 0 || x >= (int)width || y >= (int)height) 
  { return theWall; }

  return map[ y*height + x ];
}

TouchableObject* CollisionMap::atMap(int x, int y)
{
  if(x < 0 || y < 0 || x/cellInPixels >= width || y/cellInPixels >= height) 
  { return theWall; }

  return map[ (y/cellInPixels)*height + x/cellInPixels ];
}

bool CollisionMap::isEmptyCol(const SDL_Rect& rc)
{
  const int left = rc.x;
  const int top = rc.y;
  const int right = (rc.x+rc.w);
  const int bottom = (rc.y+rc.h);

  if(left < 0 || top < 0) { 
//    cout << "Warning!  rect in CollisionMap::isEmpty() had negative x or y" << endl;
    return false; 
  }
  if(right >= (int)width || bottom >= (int)height) {
//    cout << "Warning!  rect in CollisionMap::isEmpty() went outside the map (right/bottom)" << endl;
    return false; 
  }
  for(int y = top; y <= bottom; ++y) {
    for(int x = left; x <= right; ++x) {
      if(atCol(x,y)) { return false; }
    }
  }
  return true;
}

bool CollisionMap::isEmptyMap(const SDL_Rect& rc)
{
  const int left = rc.x/cellInPixels;
  const int top = rc.y/cellInPixels;
  const int right = (rc.x+rc.w)/cellInPixels;
  const int bottom = (rc.y+rc.h)/cellInPixels;

  if(left < 0 || top < 0) { 
    cout << "Warning!  rect in CollisionMap::isEmpty() had negative x or y" << endl;
    return false; 
  }
  if(right >= (int)width || bottom >= (int)height) {
    cout << "Warning!  rect in CollisionMap::isEmpty() went outside the map (right/bottom)" << endl;
    return false; 
  }
  for(int y = top; y <= bottom; ++y) {
    for(int x = left; x <= right; ++x) {
      if(atCol(x,y)) { return false; }
    }
  }
  return true;
}

void CollisionMap::addTouchableObject(TouchableObject* obj)
{
  if(!obj) { cerr << "addTouchableObject(NULL)" << endl; }

  const int bottom = obj->getBottom() / cellInPixels;
  const int right = obj->getRight() / cellInPixels;
  int index = 0;
  for(int y = obj->getTop() / cellInPixels; y <= bottom; ++y) {
    for(int x = obj->getLeft() / cellInPixels; x <= right; ++x) {
      index = (y*width) + x;
      if(index >= 0 && index < (int)(width*height) ) {
        map[ index ] = obj;
      }
      else {
        cerr << "Error in addTouchableObject.  index = " << index << endl;
      }
    }
  }
}

void CollisionMap::removeTouchableObject(TouchableObject* obj)
{
  if(!obj) { cerr << "removeTouchableObject(NULL)" << endl; }

  const int bottom = obj->getBottom() / cellInPixels;
  const int right = obj->getRight() / cellInPixels;
  int index = 0;
  for(int y = obj->getTop() / cellInPixels; y <= bottom; ++y) {
    for(int x = obj->getLeft() / cellInPixels; x <= right; ++x) {
      index = (y*width) + x;
      if(index >= 0 && index < (int)(width*height) ) {
        map[ index ] = NULL;
      }
      else {
        cerr << "Error in removeTouchableObject.  index = " << index << endl;
      }
    }
  }
}

void moveRectWithCdr(const CollisionMap& colMap,
                     const SDL_Rect& inPosBegin,
                     const SDL_Rect& inPosEnd,
                     CollisionEvent& outEvent)
{
//  cout << "CDR: ******************* BEGIN ************************" << endl;
  // if collMap.map is NULL, return fatal failure
  if(colMap.map == NULL) {
    cerr << "Error!  map pointer is NULL in moveRectWithCdr()" << endl;
    outEvent.result = sm_cdr_failure;
    return;
  }

//  cout << "CDR: colMap.height = " << colMap.height << ", width = " << colMap.width
//       << ", colMap.cellInPixels = " << colMap.cellInPixels << endl;
//  cout << "CDR: inPosBegin = " << inPosBegin.x << "," << inPosBegin.y << "," << inPosBegin.w << "," << inPosBegin.h << endl;
//  cout << "CDR: inPosEnd = " << inPosEnd.x << "," << inPosEnd.y << endl;

  // 1) determine scale factor from colmap to player rects 
  //    (based on colMap.cellInPixels)

//  Uint16 scaledWidth = inPosBegin.w/colMap.cellInPixels + 1;
//  Uint16 scaledHeight = inPosBegin.h/colMap.cellInPixels + 1;
  int scaledPosBeginX1 = inPosBegin.x / (int)colMap.cellInPixels;
  int scaledPosBeginY1 = inPosBegin.y / (int)colMap.cellInPixels;
  int scaledPosBeginX2 = (inPosBegin.x + inPosBegin.w - 1) / (int)colMap.cellInPixels;
  int scaledPosBeginY2 = (inPosBegin.y + inPosBegin.h - 1) / (int)colMap.cellInPixels;

//  cout << "CDR: scaledPosBegin = " << scaledPosBeginX1 << "," << scaledPosBeginY1 << "," << scaledPosBeginX2 << "," << scaledPosBeginY2 << endl;

  int scaledPosEndX1 = inPosEnd.x / (int)colMap.cellInPixels; 
  if(inPosEnd.x < 0) { scaledPosEndX1 -= 1; }
  int scaledPosEndY1 = inPosEnd.y / (int)colMap.cellInPixels;
  if(inPosEnd.y < 0) { scaledPosEndY1 -= 1; }
  int scaledPosEndX2 = (inPosEnd.x + inPosBegin.w - 1) / (int)colMap.cellInPixels;
  if(inPosEnd.x + inPosBegin.w - 1 < 0) { scaledPosEndX2 -= 1; }
  int scaledPosEndY2 = (inPosEnd.y + inPosBegin.h - 1) / (int)colMap.cellInPixels;
  if(inPosEnd.y + inPosBegin.h - 1 < 0) { scaledPosEndY2 -= 1; }

//  cout << "CDR: scaledPosEnd = " << scaledPosEndX1 << "," << scaledPosEndY1 << "," << scaledPosEndX2 << "," << scaledPosEndY2 << endl;

  // assume success
  outEvent.result = sm_cdr_success;
  outEvent.cdrObject = NULL;
  outEvent.cdrObjectType = (unsigned char)sm_cdo_nothing;
  outEvent.finalX = inPosEnd.x;
  outEvent.finalY = inPosEnd.y;

  // these two variables describe how many strips and how many cells are involved with the movement
  int numStrips = 0;
  int numCellsInStrip = 0;

  // these two variables are set to the values we need to increment to get
  // to the next strip and get to the next cell
  // for instance, with upward motion, nextStrip would be set to -rowInc and nextCell would be +1
  // with rightward motion, nextStrip would be set to +1 and nextCell would be +rowInc
  int nextStrip = 0;
  int nextCell = 0;

  // this is the current index we're looking at
  int currentCellIx = 0;

  // these two variables hold the adjusted values that are applied in the case of a
  // collision and we need to stop movement
  int finalOffsetX = 0;
  int finalOffsetY = 0;

  sm_direction dir = sm_dir_none;

  // 2) compare x/y in inPosBegin to inPosEnd
  //    a) determine if it was a horizontal or vertical
  //    b) otherwise, return success immediately
  if(inPosBegin.x != inPosEnd.x && inPosBegin.y == inPosEnd.y) {
//    cout << "CDR: horizontal movement" << endl;

    numCellsInStrip = scaledPosEndY2 - scaledPosEndY1 + 1;

    if(scaledPosEndX2 > scaledPosBeginX2) { 
//      cout << "CDR: Rightward movement" << endl;
      dir = sm_dir_right;

      numStrips = scaledPosEndX2 - scaledPosBeginX2;
      nextStrip = 1;
      nextCell = colMap.width;

      currentCellIx = scaledPosBeginX2 + (scaledPosBeginY1*colMap.width) + nextStrip;

      finalOffsetX = -inPosBegin.w;
      finalOffsetY = inPosBegin.y - scaledPosBeginY1 * colMap.cellInPixels;
    }
    else if(scaledPosEndX1 < scaledPosBeginX1) {
//      cout << "CDR: Leftward movement" << endl;
      dir = sm_dir_left;

      numStrips = scaledPosBeginX1 - scaledPosEndX1;
      nextStrip = -1;
      nextCell = colMap.width;

      currentCellIx = scaledPosBeginX1 + (scaledPosBeginY1*colMap.width) + nextStrip;

      finalOffsetX = colMap.cellInPixels;
      finalOffsetY = inPosBegin.y - scaledPosBeginY1 * colMap.cellInPixels;
    }
    else {
//      cout << "CDR: Movement didn't cross a colmap boundary, we don't care." << endl;
      return;
    }

  } else if(inPosBegin.y != inPosEnd.y && inPosBegin.x == inPosEnd.x) {
//    cout << "CDR: vertical movement" << endl;

    numCellsInStrip = scaledPosEndX2 - scaledPosEndX1 + 1;

    if(scaledPosEndY2 > scaledPosBeginY2) { 
//      cout << "CDR: Downward movement" << endl;
      dir = sm_dir_down;

      numStrips = scaledPosEndY2 - scaledPosBeginY2;
      nextStrip = colMap.width;
      nextCell = 1;

      currentCellIx = scaledPosBeginX1 + (scaledPosBeginY2*colMap.width) + nextStrip;

      finalOffsetX = inPosBegin.x - scaledPosBeginX1*colMap.cellInPixels;
      finalOffsetY = -inPosBegin.h;
    }
    else if(scaledPosEndY1 < scaledPosBeginY1) {
//      cout << "CDR: Upward movement" << endl;
      dir = sm_dir_up;

      numStrips = scaledPosBeginY1 - scaledPosEndY1;
      nextStrip = -(int)colMap.width;
      nextCell = 1;

      currentCellIx = scaledPosBeginX1 + (scaledPosBeginY1*colMap.width) + nextStrip;

      finalOffsetX = inPosBegin.x - scaledPosBeginX1*colMap.cellInPixels;
      finalOffsetY = colMap.cellInPixels;
    }
    else {
//      cout << "CDR: Movement didn't cross a colmap boundary, we don't care." << endl;
      return;
    }

  } else {
    cerr << "CDR: Error:  only horiz/vert movement allowed." << endl;
    outEvent.result = sm_cdr_failure;
    return;
  }

//  cout << "CDR: Num strips=" << numStrips << ", cells-in-strip=" << numCellsInStrip << endl;
//  cout << "CDR: nextStrip=" << nextStrip << ", nextCell=" << nextCell << endl;
//  cout << "CDR: currentCellIx=" << currentCellIx << endl;
//  cout << "CDR: finalOffsetX=" << finalOffsetX << ", finalOffsetY=" << finalOffsetY << endl;

  int startOfStrip = currentCellIx;
  bool bHitEdge = false;
  bool bHitSomethingElse = false;
  for(int stripnum = 0; stripnum < numStrips; ++stripnum) {
    for(int cellnum = 0; cellnum < numCellsInStrip; ++cellnum) {
//      cout << "CDR: Looking at strip #" << stripnum << ", cell# " << cellnum << ", currentCellIx=" << currentCellIx << " and map value is " << (int)colMap.map[currentCellIx] << endl;

      // determine if we've wrapped the map
      switch(dir) {
        case sm_dir_right:
          if( (currentCellIx % (int)colMap.width) == 0 || currentCellIx >= (int)(colMap.width*colMap.height)) { 
            bHitEdge = true;
          }
          break;
        case sm_dir_left:
          if( (currentCellIx % (int)colMap.width) == (colMap.width-1) || currentCellIx < 0) { 
            bHitEdge = true;
          }
          break;
        case sm_dir_down:
          if( currentCellIx >= (int)(colMap.width*colMap.height)) { 
            bHitEdge = true;
          }
          break;
        case sm_dir_up:
          if( currentCellIx < 0 ) { 
            bHitEdge = true;
          }
          break;
      }

      if(bHitEdge) { 
//        cout << "CDR: We hit the edge of the map" << endl; 
        outEvent.cdrObject = NULL;
        outEvent.cdrObjectType = (unsigned char)sm_cdo_edge;
      }

      // if we didn't hit the edge, then see if we hit something else
      if(!bHitEdge && colMap.map[currentCellIx]) {
        outEvent.cdrObject = colMap.map[currentCellIx];
        outEvent.cdrObjectType = colMap.map[currentCellIx]->getObjectType();
//        cout << "CDR: Found a collision with a '" << (int)outEvent.cdrObjectType << "'" << endl;
        bHitSomethingElse = true;
      }

      if(bHitEdge || bHitSomethingElse) {
        // calculate from start of this impassable strip where the new final position is
        outEvent.result = sm_cdr_partial_move;
        outEvent.finalX = (startOfStrip % colMap.width) * colMap.cellInPixels + finalOffsetX;
        outEvent.finalY = (startOfStrip / colMap.width) * colMap.cellInPixels + finalOffsetY;

        // if we've wrapped from the right edge to the left edge, let's correct that
        if(outEvent.finalX < 0) { 
          outEvent.finalX += colMap.width*colMap.cellInPixels;
          outEvent.finalY -= colMap.cellInPixels;
        }
        // if we've wrapped from the left edge to the right edge, let's correct that
        else if(outEvent.finalX >= (int)(colMap.width*colMap.cellInPixels)) {
          outEvent.finalX -= colMap.width*colMap.cellInPixels;
          outEvent.finalY += colMap.cellInPixels;
        }
        // if we tried to go "up" past the top edge of the map, let's correct that
        if(startOfStrip < 0 && dir == sm_dir_up) {
          outEvent.finalY = 0;
          outEvent.finalX = inPosBegin.x;
        }

        if(outEvent.finalX == inPosBegin.x && outEvent.finalY == inPosBegin.y) {
          outEvent.result = sm_cdr_cannot_move;
        }

//        cout << "CDR: finalX = " << outEvent.finalX << ", finalY = " << outEvent.finalY << endl;

        return;
      }
      currentCellIx += nextCell;
    }
    currentCellIx += nextStrip;
    startOfStrip = currentCellIx;
  }

//  cout << "CDR: finalX = " << outEvent.finalX << ", finalY = " << outEvent.finalY << endl;

  return;
}
